

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "common.h"
#include "clock.h"

#ifdef USE_DISP
	#include "disp.h"
#endif
#ifdef USE_PCA
	#include "pca.h"
#endif
#ifdef USE_SFB
	#include "speedfeedback.h"
#endif
#ifdef USE_NVS
	#include "nonVolatileStorage.h"
#endif

#include "serial.h"
#include "serialDataMan.h"

#ifndef SDCC
uchar xdata mempoll[HEAP_SIZE];
#endif

char vehicleSteering=0;
char vehicleGear=0;

static volatile uint cmnTimer=0;

void getCtrlDataCallback(uchar *pointer, uchar len)
{
	static bool isConnected=false;
	static bool isDispCurrentConnectInfo=false;

	if(len>0) {

#ifdef USE_DISP
		if(!isConnected) {
			DISP_SET_S(1,3,"lean your phone to control...");
			isConnected=true;
			isDispCurrentConnectInfo=false;
		}
#endif
		{
#define ARROW_THRESHOLD 7
#define ARROW_DISP_BANK 0
			char gear=*pointer++;
			char dir=*pointer++;
			uchar t=*pointer++;

			if(gear==0 && dir==0 && t==0xc3) {
					CLK_DELAY(3000);
					ISP_CONTR=0x60;
					return;
			}
			else if(t==0xc0){
				NVS_GET()->forward_dir=dir;
				NVS_STORE();
			}
			else if(t==0xc1){
				if(NVS_LOAD()) {
					SERIAL_SEND_C(NVS_GET()->forward_dir);
				}
			}
			else if(t==0xa0){
				uint addr;
				addr=gear&0xff;
				addr<<=8;
				addr|=dir&0xff;
				NVS_OPT_BYTE(&dir,addr,NVS_WRITE);
			}
			else if(t==0xa1){
				uchar dat;
				uint addr;
				addr=gear&0xff;
				addr<<=8;
				addr|=dir&0xff;
				NVS_OPT_BYTE(&dat,addr,NVS_READ);
				SERIAL_SEND_C(dat);
			}
			else if(t==0xa2){
				uint addr;
				addr=gear&0xff;
				addr<<=8;
				addr|=dir&0xff;
				NVS_OPT_BYTE(&dir,addr,NVS_ERASE);
			}
			else if(t==0xa3){
				SERIAL_SEND_C((NVS_ADDR()>>8)&0xff);
				SERIAL_SEND_C(NVS_ADDR()&0xff);
			}

			vehicleGear=gear;
			vehicleSteering=dir;//update dirction controller

//			PCA_SETBRAKE(t&0xf0);
//			test=t&0x0f;

#ifdef USE_DISP
			if(gear<-ARROW_THRESHOLD) {
				if(dir<-ARROW_THRESHOLD) {
					DISP_SET_ARROW(ARROW_DISP_BANK,1);
				}
				else if(dir>ARROW_THRESHOLD) {
					DISP_SET_ARROW(ARROW_DISP_BANK,7);
				}
				else {
					DISP_SET_ARROW(ARROW_DISP_BANK,0);
				}
			}
			else if(gear>ARROW_THRESHOLD) {
				if(dir<-ARROW_THRESHOLD) {
					DISP_SET_ARROW(ARROW_DISP_BANK,3);
				}
				else if(dir>ARROW_THRESHOLD) {
					DISP_SET_ARROW(ARROW_DISP_BANK,5);
				}
				else {
					DISP_SET_ARROW(ARROW_DISP_BANK,4);
				}
			}
			else {
				if(dir<-ARROW_THRESHOLD) {
					DISP_SET_ARROW(ARROW_DISP_BANK,2);
				}
				else if(dir>ARROW_THRESHOLD) {
					DISP_SET_ARROW(ARROW_DISP_BANK,6);
				}
				else {
					DISP_SET(ARROW_DISP_BANK,' ');
				}
			}
#endif
		}
	}
	else {
#ifdef USE_DISP
		if(!isDispCurrentConnectInfo) {
			isDispCurrentConnectInfo=true;
			if(isConnected) {
				DISP_SET_S(0,3,"Connection lost");
				isConnected=false;
			}
			else {
				DISP_SET_S(0,3,"Waiting connection...");
			}
		}
#endif
#ifdef USE_PCA
		PCA_SETFREE();
#endif
	}
	SDM_REGISTER(SDM_FRAMETYPE_FIXLEN, 3, getCtrlDataCallback, &cmnTimer, 500);
	return;
}

#ifdef RECONFIG_BT
static volatile uint btInitTimer=0;
#define BTCONFTIMEOUTRETRY 500	//500ms
#define BTCONFTIMEOUTSKIP 2000	//2s
#ifdef MARCO_TO_FUNCTION
void btConfCallback(uchar *pointer, uchar len);
void PROCESS_BTCONFCMD(uchar *state, uchar *p, uchar len, uchar *cmd, uchar *response) {
	if(len>0 && !strncmp(p,response,strlen(response))) {
		SDM_REGISTER(SDM_FRAMETYPE_FIXLEN, 0, btConfCallback, &btInitTimer, 1);
		(*state)++;
	}
	else {
		SERIAL_FLUSH_GET();
		SERIAL_SEND_S(cmd);
		SDM_REGISTER(SDM_FRAMETYPE_FIXLEN, strlen(response), btConfCallback, &btInitTimer, BTCONFTIMEOUTRETRY);
	}
}
#else
#define PROCESS_BTCONFCMD(STATE, POINTER, LEN, CMD, RESPONSE) \
		if(len>0 && !strncmp(pointer,RESPONSE,strlen(RESPONSE))) { \
			state++; \
			SDM_REGISTER(SDM_FRAMETYPE_FIXLEN, 0, btConfCallback, &btInitTimer, 1); \
		} \
		else { \
			SERIAL_FLUSH_GET(); \
			SERIAL_SEND_S(CMD); \
			SDM_REGISTER(SDM_FRAMETYPE_FIXLEN, strlen(RESPONSE), btConfCallback, &btInitTimer, BTCONFTIMEOUTRETRY); \
		}
#endif
void btConfCallback(uchar *pointer, uchar len)
{
	static enum {
		BTCONF_AT=0,
		BTCONF_SET,
		BTCONF_NAME,
		BTCONF_CLASS,
		BTCONF_BAUD,
		BTCONF_STATE0,
		BTCONF_TOTAL,
	} state=BTCONF_AT;

	switch(state) {
	case BTCONF_AT:
		if(cmnTimer>BTCONFTIMEOUTRETRY) {
			SDM_REGISTER(SDM_FRAMETYPE_FIXLEN, 3, getCtrlDataCallback, &cmnTimer, 500);
		}
		else {
			PROCESS_BTCONFCMD(&state,pointer,len,"AT","OK");
		}
		break;
	case BTCONF_SET:
		PROCESS_BTCONFCMD(&state,pointer,len,"AT+SETTING","OK+STATE:1");
		break;
	case BTCONF_NAME:
		PROCESS_BTCONFCMD(&state,pointer,len,"AT+NAME=61Tech","OK");
		break;
	case BTCONF_CLASS:
		PROCESS_BTCONFCMD(&state,pointer,len,"AT+CLASS=0x000808","OK");
		break;
	case BTCONF_BAUD:
		//PROCESS_BTCONFCMD("AT+BAUD=5","OK");
		if(len>0 && !strncmp(pointer,"OK",strlen("OK"))) {
			state++;
			SDM_REGISTER(SDM_FRAMETYPE_FIXLEN, 0, btConfCallback, &btInitTimer, 1);
		}
		else {
			SERIAL_BAUD_X12(0);
			SERIAL_FLUSH_GET();
			SERIAL_SEND_S("AT+BAUD=5");
			while(SERIAL_ISSENDING()) {;}//we can change baud rate only after the sending work is finished
			SERIAL_BAUD_X12(1);
			SDM_REGISTER(SDM_FRAMETYPE_FIXLEN, strlen("OK"), btConfCallback, &btInitTimer, BTCONFTIMEOUTRETRY);
		}
		break;
	case BTCONF_STATE0:
		PROCESS_BTCONFCMD(&state,pointer,len,"AT+STATE0","OK+STATE:0");
		break;
	case BTCONF_TOTAL:
		SDM_REGISTER(SDM_FRAMETYPE_FIXLEN, 3, getCtrlDataCallback, &cmnTimer, 1);
		return;
	default:
		break;
	}
#undef PROCESS_BTCONFCMD
#ifdef USE_DISP
	DISP_SET(3,BTCONF_TOTAL-state+'0');
#endif
	return;
}
#endif

void main()
{
	//P2M1|=0x80;
	//P3M1|=0x80;
	//P2&=0xfc;

#ifndef SDCC
	init_mempool(mempoll,HEAP_SIZE);
#endif

	CLK_INIT();
#ifdef USE_DISP
	DISP_INIT();
#endif
	SERIAL_INIT();
	SDM_INIT();
#ifdef USE_PCA
	PCA_INIT();
#endif
#ifdef USE_SFB
	SFB_INIT();
#endif

	CLK_ENABLE();
#ifdef USE_DISP
	DISP_ENABLE();
#endif
	SERIAL_ENABLE();
#ifdef USE_PCA
	PCA_ENABLE();
#endif
#ifdef USE_SFB
	SFB_ENABLE();
#endif
	
	EA=1;//enable interrupt

#ifdef USE_NVS
	if(NVS_INIT()) {
		if(!NVS_LOAD()) {
			//if nvs is damaged, NVS_LOAD fail
			//if nvs has no data, NVS_LOAD fail too
			if(NVS_ISVALID()) {
				//if(nvs has no data, store the new data
				//a.clear
				NVS_CLEAR();
				//b.fill data
				NVS_GET()->forward_dir=0;
				//c.store
				NVS_STORE();
			}
		}
	}
#endif

#ifdef RECONFIG_BT
#ifdef USE_DISP
	DISP_SET_S(0,2,"Initializing...");
	DISP_RACE_SPEED(20);
#endif
	//do bluetooth initialization
	CLK_ADDCLK(&cmnTimer);
	CLK_ADDCLK(&btInitTimer);
	SDM_REGISTER(SDM_FRAMETYPE_FIXLEN, 0, btConfCallback, &btInitTimer, 1);
#else
	SDM_REGISTER(SDM_FRAMETYPE_FIXLEN, 3, getCtrlDataCallback, &cmnTimer, 500);
#endif

	while(1)
	{
#ifdef USE_DISP
		DISP_QUERY();
#endif
		SDM_QUERY();
		
	}



}
